#include "sleep_state.h"
#include "fsm.h"
#include <iostream>
using namespace std;
using namespace arm;
#include "masters/arm_controller.h"

SleepState::SleepState()
{

}

void SleepState::Update(FSM *fsm)
{
    //cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" SleepState::Update"<<endl;
    char master_device=fsm->arm_controller_->GetMasterDeviceIndex();
    //if master device allocated, update its connection status in every loop
    //however, if it's not allocated with device , or the device is out of connection, omit this step


    if(fsm->arm_controller_->GetRunningStatus()==main_controller::kRCWorking)
        fsm->SetState(BaseState::kPrepareState);


    //cout<<"idle state"<<endl;
//    switch(fsm->arm_controller_->GetExternOrder())
//    {
//    case 'g':case 'G':
//        //fsm->cur_state_->OnExit(fsm);
//        fsm->SetState(BaseState::ReadyState);
//        //fsm->cur_state_->OnEntry(fsm);
//        break;
//    case 'q':case 'Q':
//        fsm->arm_controller_->SetStop(true);
//        fsm->cur_state_->OnExit(fsm);
//        break;
//    default:
//        break;
//    }
//    fsm->arm_controller_->SetExternOrder(' ');

}

void SleepState::OnEntry(FSM *fsm)
{
    //fsm->arm_controller_->StopRobotRC();
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" SleepState::OnEntry"<<endl;
    char master_device= fsm->arm_controller_->GetMasterDeviceIndex();
    if(master_device<0) //which means no device allocated or the certain device is out of connection
        return;
    else                //if there is a device, clear force control
    {
//        drdRegulatePos(false,master_device);
//        drdRegulateGrip(false,master_device);
//        dhdSetForceAndGripperForce(0,0,0,0,master_device);
    }

    //drdStop(true,fsm->arm_controller_->GetMasterDeviceIndex());
    //drdClose(fsm->arm_controller_->GetMasterDeviceIndex());

//    if(fsm->arm_controller_->SetMasterForce(Eigen::Vector3d::Zero()))
//        cout<<"failed to clear feedback force"<<endl;

}

void SleepState::OnExit(FSM *fsm)
{
    cout<<slave_names_[fsm->arm_controller_->GetSlaveTargetIndex()]<<" SleepState::OnExit"<<endl;
}



